#include "pinocchio/fwd.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "class03/CartpoleDynamics.h"
#include "ros/ros.h"//因为要访问ros文件信息
#include "ocs2_core/misc/LoadData.h"

CartpotDynamics::CartpotDynamics(const PinocchioInterface &pinocchioInterface)
{
    pinocchioInterfaceAdPtr_ = std::make_shared<ocs2::PinocchioInterfaceCppAd>(
        pinocchioInterface.toCppAd());
    ros::NodeHandle nh;//ros的代理人，可以处理ros的订阅或者发布，每个文件都可以有一个，不必要在文件之间传递
    std::string config;
    nh.getParam("config_file",config);//拿到config路径
    int stateDim = 0;
    int inputDim = 0;

    loadData::loadCppDataType(config, "StateDim", stateDim);
    loadData::loadCppDataType(config, "InputDim", inputDim);
    std::string modelFolder;
    bool recompileLibraries = false;
    loadData::loadCppDataType(config, "modelSettings.modelFolder", modelFolder);
    loadData::loadCppDataType(config, "modelSettings.recompileLibraries", recompileLibraries);
    
    initialize(stateDim,inputDim,"cartpole_dynamics",modelFolder,recompileLibraries);//动态链接库
}

CartpotDynamics *CartpotDynamics::clone() const
{
    return new CartpotDynamics(*this);
}

ad_vector_t CartpotDynamics::systemFlowMap(ad_scalar_t time,
                                           const ad_vector_t &state,
                                           const ad_vector_t &input,
                                           const ad_vector_t &parameters) const
{
    //dx=f(x,u)
    //state     [p \thetal1\thetal2\thetal3  v omegal1\omegal2\omegal3]
    //input     [f]
    //q         [p \thetal\thetal2\thetal3]
    //dq        [v \omegal1\omegal2\omegal3]
    //input     [f 0 0]
    //tau 力矩
    //nv 速度量个数
    //nq 关节量

    const auto &model=pinocchioInterfaceAdPtr_->getModel();
    auto &data=pinocchioInterfaceAdPtr_->getData();

    ad_vector_t q=state.head(model.nq);//前nq
    ad_vector_t dq=state.tail(model.nv);//后dq个
    ad_vector_t tau=ad_vector_t::Zero(model.nv);
    tau[0]=input[0];

    ad_vector_t ddq=pinocchio::aba(model,data, q,dq,tau);

    ad_vector_t dx(state.size());
    dx << dq, ddq;//dx,

    return dx;
}
